/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-18 20:36:49
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-20 09:06:11
 * @FilePath: /esp32_motor/motor/algorithm /closeLoop.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "currentSensing.h"
#include "fluxObserver.h"
#include "motor.h"
#include "esp_timer.h"
#include "math_common.h"
#include "motor_priv.h"
#include "esp_log.h"
#include <math.h>
#include <string.h>
#include "motor_common.h"
#include "adc.h"
#include "smbObserver.h"

static const _iq21 _180_pi  = _IQ21(180.F / _PI);
static const _iq24 _0_1     = _IQ(0.01);
static char       *tag      = "closeLoop";
_iq24              iqTarget = 0;
_iq24              iqError  = 0;
_iq24              idError  = 0;
_iq24              uq;
_iq24              ud;
_iq21              speedError;
long               time1, time2;

/*闭环状态更新*/
static void MOTOR_dataFrameUpdate(MotorDev *dev)
{
    MotorObj      *priv = NULL;
    DataSendFrame *frame;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv  = dev->priv;
    frame = &priv->dataFrame;
    /*赋值*/
    frame->fData[0] = _IQ21toF(speedError);
    frame->fData[1] = _IQtoF(iqTarget);
    frame->fData[2] = _IQtoF(iqError);
    frame->fData[3] = _IQtoF(uq);
    frame->dataNum  = 4;
    /*发送当前的状态到上位机*/
    xQueueSendFromISR(dataQueue, &priv->dataFrame, 0);
}

void IRAM_ATTR MOTOR_closeLoop(MotorDev *dev)
{
    MotorObj *priv = NULL;
    // AS5600Dev *sensor = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv = dev->priv;
    if (!priv->sensor)
    {
        ESP_LOGE(tag, "the sensor is not register, can not close loop\n");
        return;
    }
    time1 = esp_timer_get_time();
    /*获取当前的iq,id*/
    MOTOR_updateAmpere(dev);
    ClarkTrans(&priv->ampare);
    MOTOR_locationUpdate(dev);
    FastSinCos(priv->curAngle.angle, &priv->curAngle.valueSinCos);
    ParkTrans(&priv->ampare, priv->curAngle.valueSinCos);
    priv->ampare.q = priv->iqFilter->lowPassFilter(priv->iqFilter, priv->ampare.q);
    priv->ampare.d = priv->idFilter->lowPassFilter(priv->idFilter, priv->ampare.d);
    // /*计算iq,id误差值*/
    // iqTarget       = priv->setSpeed;
    iqError = iqTarget - priv->ampare.q;
    idError = -priv->ampare.d;
    /*获取期望uq,ud*/
    uq = priv->iqPID->getRefValue(priv->iqPID, iqError);
    ud = priv->idPID->getRefValue(priv->idPID, idError);
    if (uq > 0)
    {
        uq = _IQsat(uq, uq, _0_1);
    }
    else
    {
        uq = _IQsat(uq, -_0_1, uq);
    }

    /*uq的方向应跟速度方向保持一致*/
    // uq = priv->setSpeed > 0 ? MAX(uq, -0.2F) : MIN(uq, 0.2F);
    dev->setUq(dev, uq);
    dev->setUd(dev, ud);
    // printf("%f,%f,%f,%f,%f,%f\n",speedError,iqTarget,iqError,idError,uq,ud);
    ParkReversalTrans(&priv->voltage, priv->curAngle.valueSinCos);
    ClarkReversalTrans(&priv->voltage);
    priv->setPhaseVoltage(dev);
    /*计算速度误差值*/
    speedError = priv->setSpeed - priv->realSpeed;
    iqTarget   = priv->speedPID->getRefValue(priv->speedPID, _IQ21toIQ(speedError));
    time2      = esp_timer_get_time();
    MOTOR_dataFrameUpdate(dev);
}